#include <assert.h>
#include "orscuAlgIHSFusion.cuh"
#include "cuda_runtime.h"
#include "device_launch_parameters.h"

#define PI 3.1415926535

__device__ void cuRGBtoIHS(double R, double G, double B, double &I, double &H, double&S);
__device__ void cuIHStoRGB(double I, double H, double S, double &R, double &G, double &B);

bool InitCUDA(){

	int	count;

	cudaGetDeviceCount(&count) ;

	if ( count == 0)
	{
		fprintf(stderr, "There is no device.\n") ;

		return false;
	}

	int	 i;

	for ( i = 0 ;i < count ; i++)
	{
		cudaDeviceProp prop ;

		if ( cudaGetDeviceProperties(& prop ,i) == cudaSuccess)
		{
			if ( prop.major >= 1)
			{
				break;
			}
		}
	}
	if ( i == count)
	{
		fprintf(stderr ,"There is no device supporting CUDA1.x.\n");

		return false;
	}

	cudaSetDevice( i );

	return true;

}

__global__ void cuIHSFusion(float* dev_bBuf, float* dev_gBuf, float* dev_rBuf,float* dev_panBuf ,float m_a1,float m_a0,int wid,int hei){
	
	int x = threadIdx.x + blockDim.x*blockIdx.x;
	int y = threadIdx.y + blockDim.y*blockIdx.y;
	
	int offset = x + y*blockDim.x*gridDim.x;
	//////////////////////////////////////////////////////////////////////////

	double I, H, S;
	double r,g,b, p;

	p = m_a0 + m_a1 * dev_panBuf[offset];
	if( p < 0 )
		p = 0;

	cuRGBtoIHS( dev_rBuf[offset], dev_gBuf[offset] , dev_bBuf[offset], I, H, S );

	cuIHStoRGB( p, H, S, g, b, r );

	dev_rBuf[offset] = r;
	dev_gBuf[offset] = g;
	dev_bBuf[offset] = b;
}

 void IHSFusion(float* m_bBuf,float* m_gBuf, float* m_rBuf,float* m_panBuf , int size,float m_a1,float m_a0, int wid,int hei){

	dim3 threadsPerBlock(THREAD_NUM,THREAD_NUM);
	dim3 blocksPerGrid( (wid+threadsPerBlock.x-1)/threadsPerBlock.x,(hei+threadsPerBlock.y-1)/threadsPerBlock.y);

	float* dev_bBuf = NULL;
	float* dev_gBuf = NULL;
	float* dev_rBuf = NULL;
	float* dev_panBuf = NULL;
	
	cudaError_t err ;
	err = cudaMalloc((void**)&dev_bBuf, size);

	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda malloc dev_bBuf failed",cudaGetLastError());
	}
	err = cudaMalloc( (void**)&dev_rBuf,size);
	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda malloc dev_rBuf failed",cudaGetLastError());
	}
	err = cudaMalloc( (void**)&dev_gBuf, size);
	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda malloc dev_gBuf failed",cudaGetLastError());
	}
	err = cudaMalloc( (void**)&dev_panBuf, size	);

	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda malloc dev_pBuf failed",cudaGetLastError());
	}
	//// cuda memcpy
	err = cudaMemcpy(dev_bBuf, m_bBuf, size, cudaMemcpyHostToDevice);
	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda memcpy dev_bBuf failed",cudaGetLastError());
	}
	err = cudaMemcpy( dev_gBuf, m_gBuf, size,cudaMemcpyHostToDevice);
	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda memcpy dev_gBuf failed",cudaGetLastError());
	}
	err = cudaMemcpy( dev_rBuf, m_rBuf, size,cudaMemcpyHostToDevice);
	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda memcpy dev_rBuf failed",cudaGetLastError());
	}
	err = cudaMemcpy( dev_panBuf, m_panBuf, size,cudaMemcpyHostToDevice);

	if ( err != cudaSuccess )
	{
		fprintf(stderr, "cuda malloc dev_pBuf failed",cudaGetLastError());
	}
	cuIHSFusion<<<blocksPerGrid,threadsPerBlock,0>>>( dev_bBuf,  dev_gBuf,  dev_rBuf, dev_panBuf , m_a1, m_a0, wid, hei);
	
	err = cudaMemcpy( m_bBuf, dev_bBuf,size, cudaMemcpyDeviceToHost);
	err = cudaMemcpy( m_gBuf, dev_gBuf,size, cudaMemcpyDeviceToHost);
	err = cudaMemcpy( m_rBuf, dev_rBuf,size, cudaMemcpyDeviceToHost);

	cudaFree( dev_rBuf);
	cudaFree( dev_gBuf);
	cudaFree( dev_bBuf);
}

__device__ void cuIHStoRGB(double I, double H, double S, double &R, double &G, double &B)
 {
	 if (H>=0&&H<2*PI/3)
	 {
		 R = I*(1+S*cos(H)/cos(PI/3-H));		
		 B = I*(1-S);
		 G = 3*I-B-R;
	 }
	 else if (H>=2*PI/3&&H<4*PI/3)
	 {
		 G = I*(1+S*cos(H-2*PI/3)/cos(PI-H));
		 R = I*(1-S);
		 B = 3*I-G-R;
	 }
	 else if (H>=4*PI/3&&H<2*PI)
	 {
		 B = I*(1+S*cos(H-4*PI/3)/cos(5*PI/3-H));
		 G = I*(1-S);
		 R = 3*I-G-B;
	 }
 }

__device__ void cuRGBtoIHS(double R, double G, double B, double &I, double &H, double&S)
{
	double Gmin, Gmax;
	Gmin = min(R, min(G, B));
	Gmax = max(R, max(G, B));

	I = (R+G+B)/3;

	if (Gmax-Gmin < 0.001)
	{
		S = 0.0;
		H = 0.0;
	}
	else
	{
		S = 1-3/(R+G+B)*Gmin;
		double H1 = R-(G+B)/2;
		double H2 = sqrt((R-G)*(R-G)+(R-B)*(G-B));
		double H3 = acos(H1/H2);
		if (B>G)
		{
			H = 2*PI-H3;
		}
		else
		{
			H = H3; 
		}
	}
}